package GUI;

import process.Process;
import java.awt.*;
import java.awt.event.*;
import javax.swing.*;
import java.util.*;

/**
* Class Steering, simulates the steering of a vehicle.
*/
public class Steering extends JPanel implements Process
{
	private Vector obstacleVector;                  // the vector which keeps the obstacles
	private final int WIDTH = 700, HEIGHT = 700;    // the width and heigth of the scene

	private double rWidth;      // the width of the robot
	private double rLength;     // the length of the robot
	private double rX;          // the x-coordinate of the robot
	private double rY;          // the y-coordinate of the robot
	private double rTheta;      // the theta-coordinate of the robot

	private double velocity = 20;
	private double steeringAngle = 0;

	private boolean traceOn = false;

	private class Trace
	{
		public double X;
		public double Y;
		public double Theta;
		public Trace next;

		public Trace(double x, double y, double theta)
		{
			X = x;
			Y = y;
			Theta = theta;
			next = null;
		}
	}

	private Trace traceList = null;

	private void addTrace(Trace trace)
	{
		if(traceList == null)
		{
			traceList = trace;
		}
		else
		{
			Trace cursor;
			for(cursor = traceList; cursor.next != null; cursor = cursor.next);
			cursor.next = trace;
		}
	}

	/** Switches trace of the robot on and off.
	*
	* @param traceOn a boolean; if traceOn is true, the the trace is switched on.
	*/
	public void setTrace(boolean traceOn)
	{
		this.traceOn = traceOn;
	}

	// class describing simple obstacles
	private class Obstacle
	{
		public double xmin;
		public double xmax;
		public double ymin;
		public double ymax;

		public Obstacle(double x1, double x2, double y1, double y2)
		{
			xmin = x1;
			xmax = x2;
			ymin = y1;
			ymax = y2;
		}
	}

	/** Creates the world. The trace is switched off.
	*/
	public Steering()
	{
		obstacleVector = new Vector();
		setSize(WIDTH,HEIGHT);

		addObstacle(-300,-40,-280,-240);
		addObstacle(40,300,-280,-240);
		addRobot(20,30,-150,250,-Math.PI/2);

		setTrace(false);

	}

	// add an obstacle to the world
	private void addObstacle(double xmin, double xmax, double ymin, double ymax)
	{
		if(xmax < xmin || ymax < ymin) return;
		Obstacle newObst = new Obstacle(xmin,xmax,ymin,ymax);
		obstacleVector.addElement(newObst);
	}

	/**
	* Adds a robot to the world.
	*
	* @param width	a double, denoting the width of the simulated robot
	* @param length	a double, denoting the length of the simulated robot
	* @param x		a double, denoting the initial x coordinate of the robot
	* @param y		a double, denoting the initial y coordinate of the robot
	* @param theta	a double, denoting the initial angle of the robot relative to the x-axis; counter clockwise
	* has a positive sign
	*/
	public void addRobot(double width, double length, double x, double y, double theta)
	{
		rWidth = width;
		rLength = length;
		rX = x;
		rY = y;
		rTheta = theta;
		Trace newTrace = new Trace(rX,rY,rTheta);
		addTrace(newTrace);
	}

	// class needed to repaint the scene
    @Override
	public void paintComponent(Graphics g)
	{
		super.paintComponent(g);
		setBackground(Color.white);
		g.setColor(getForeground());
		drawScene(g);
	}

	// draw the scene
	private void drawScene(Graphics g)
	{
		drawObstacles(g);
		drawTrace(g);
	}

	private void drawTrace(Graphics g)
	{
		for(Trace cursor = traceList; cursor != null; cursor= cursor.next)
		{
			if(traceOn || (!traceOn && cursor.next == null))
			{
				rX = cursor.X;
				rY = cursor.Y;
				rTheta = cursor.Theta;
				drawRobot(g);
			}
		}
	}

	// draw the obstacles in the scene
	private void drawObstacles(Graphics g)
	{
		Enumeration enumVect = obstacleVector.elements();
		while(enumVect.hasMoreElements())
		{
			Obstacle obst = (Obstacle) enumVect.nextElement();
			drawSingleObstacle(g,obst);
		}
	}

	// draw a single obstale
	private void drawSingleObstacle(Graphics g, Obstacle obst)
	{
		drawStraightLine(g,obst.xmin,obst.ymin,obst.xmin,obst.ymax);
		drawStraightLine(g,obst.xmin,obst.ymax,obst.xmax,obst.ymax);
		drawStraightLine(g,obst.xmax,obst.ymax,obst.xmax,obst.ymin);
		drawStraightLine(g,obst.xmax,obst.ymin,obst.xmin,obst.ymin);
	}

	// draw the robot
	private void drawRobot(Graphics g)
	{
		double rAlpha = Math.atan(rWidth / rLength);
		double rHaDiag = Math.sqrt(Math.pow(rWidth,2) + Math.pow(rLength,2));

		double x1 = rX + rHaDiag * Math.cos(rTheta + rAlpha);
		double y1 = rY + rHaDiag * Math.sin(rTheta + rAlpha);

		double x2 = rX + rHaDiag * Math.cos(rTheta - rAlpha);
		double y2 = rY + rHaDiag * Math.sin(rTheta - rAlpha);

		double x3 = rX - rHaDiag * Math.cos(rTheta + rAlpha);
		double y3 = rY - rHaDiag * Math.sin(rTheta + rAlpha);

		double x4 = rX - rHaDiag * Math.cos(rTheta - rAlpha);
		double y4 = rY - rHaDiag * Math.sin(rTheta - rAlpha);

		drawStraightLine(g,x1,y1,x2,y2);
		drawStraightLine(g,x2,y2,x3,y3);
		drawStraightLine(g,x3,y3,x4,y4);
		drawStraightLine(g,x4,y4,x1,y1);
	}

	// draw a straight line
	private void drawStraightLine(Graphics g, double x1, double y1, double x2, double y2)
	{
		int xa = (int)x1 + WIDTH / 2;
		int xb = (int)x2 + WIDTH / 2;
		int ya = HEIGHT / 2 - (int)y1;
		int yb = HEIGHT / 2 - (int)y2;

		g.drawLine(xa,ya,xb,yb);
	}

	private double calculateDistance()
	{
		return rY + 245;
	}

	private double calculateDeviation()
	{
		return rX;
	}

	private double calculateInclination()
	{
		return rTheta;
	}

	private void printVariable(String name, double value)
	{
		System.out.print(name);
		System.out.print(" = ");
		System.out.println(value);
	}

	/** The variables deviation and inclination can be asked for.
	* The deviation is the distance to the middle of the alley in which the vehicle should be steered.
	* The deviation is measured in pixels (between approximately -350 and 350).
	* The inclination (see module work book) is measured in radials.
	* Please note, that the inclination is the angle between the axis of the robot and the X-axis,
	* which means that if the inclination equals -PI/2, the robot is moving "straight downwards".
	*/
	public double getVariable(String name)
	{
		if(name.equals("deviation")) return calculateDeviation();
		if(name.equals("inclination")) return calculateInclination();
		return 0;
	}

	/** The variable steeringAngle can be set.
	* The steering angle is measured in radials.
	*/
	public void setVariable(String name, double value)
	{
		if(name.equals("steeringAngle")) steeringAngle = value; else
		System.out.println("ERROR: attempt to set unknown variable");
	}

	public void simulateStep(double deltaT)
	{
		rX = rX + Math.cos(rTheta+steeringAngle) * velocity * deltaT;
		rY = rY + Math.sin(rTheta+steeringAngle) * velocity * deltaT;
		rTheta = rTheta + Math.sin(steeringAngle)/rLength * velocity * deltaT;
		Trace newTrace = new Trace(rX,rY,rTheta);
		addTrace(newTrace);
		repaint();
	}
}